// Road.cpp: implementation of the Road class.
//
//////////////////////////////////////////////////////////////////////

#include "Road.h"

#include <GL/glut.h>
#include <gl/glaux.h> // Header file for the GLaux library.  
#include <iostream.h>
#include <STRING.h>
#include <math.h>

static float PI = 3.14159265358979;
static float SEGMENT =10;
//#define ROW 5
//#define COL 5

//static float *controls;

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

Road::Road()
{
	this->RWidth = 10.0;
	this->RType = 0;
	this->NumberOfPoint =0;
}

Road::Road(int width,int type,int number)
{
	this->RWidth = width;
	this->RType = type;
	this->NumberOfPoint = number;
}

Road::Initial(int width,int type,int number,float *control,float WSection,float HSection,float *leftcontrols,float *rightcontrols)
{
	this->RWidth = width;
	this->RType = type;
	this->NumberOfPoint = number;
	this->VControls = control;
	this->HightSection = HSection;
	this->WidthSection = WSection;
	this->LeftControls = leftcontrols;
	this->RightControls = rightcontrols; 
}

Road::~Road()
{

}

//////////////////////////////////////////////////////////////////////
// Render
//////////////////////////////////////////////////////////////////////
/*Road::importControlPoints(char *nfile)
{
	this->NFControlpoints = nfile;
}*/

Road::importControlPoints(float *nfcontrols)
{
	this->VControls = nfcontrols;
}

Road::drawRoad()
{
	int i;
	float j;

	float x=0,y=0,z=0;
  
	float degree=0;

	float XLeft,YLeft,ZLeft;
	float XRight,YRight,ZRight;

	//this->importControlPoints(this->NFControlpoints);

	//Left Side & Right
	for (i = 0; i < this->NumberOfPoint; i++)
	{
		if(i!=(this->NumberOfPoint-1))
		{
			degree = this->findDegree(this->VControls[3*i],this->VControls[3*i+1],this->VControls[3*(i+1)],this->VControls[3*(i+1)+1]);
			//cout<<"Degree :"<<degree<<endl;
		}
		if(degree != -999)
		{	
			this->LeftControls[3*i] = this->VControls[3*i]+(this->RWidth/2)*(cos((degree+90)*PI/180));
			this->LeftControls[3*i+1] = this->VControls[3*i+1]+(this->RWidth/2)*(sin((degree+90)*PI/180));
			this->LeftControls[3*i+2] = 0.5;//this->VControls[3*i+2];

			this->RightControls[3*i] = this->VControls[3*i]+(this->RWidth/2)*(cos((degree-90)*PI/180));
			this->RightControls[3*i+1] = this->VControls[3*i+1]+(this->RWidth/2)*(sin((degree-90)*PI/180));
			this->RightControls[3*i+2] = 0.5;//this->VControls[3*i+2];
		}else
		{
			cout << "Degree Error At Road" << endl;
		}
	}

	glPushMatrix();
/*
	  // Draw yellow control points.
    glColor3f(1.0, 1.0, 0.0);
		glPointSize(5.0);
    glBegin(GL_POINTS);
      for (i = 0; i < ROW; i++)
         for (j = 0; j < COL; j++)
            glVertex3fv(controls[i][j]);
    glEnd();

		// Specify and enable the Bezier surface.
    glMap2f(GL_MAP2_VERTEX_3, 0, 1, 3, COL, 0, 1, 3*COL, ROW, controls[0][0]); 
    glEnable(GL_MAP2_VERTEX_3); 

    // Make a mesh approximation of the Bezier surface.
    glColor3f (1.0, 1.0, 1.0);
    glMapGrid2f(20, 0.0, 1.0, 20, 0.0, 1.0);
    glEvalMesh2(GL_LINE, 0, 20, 0, 20);
*/
	// Draw yellow control points.
    glColor3f(1.0, 0.0, 0.0);
		glPointSize(5.0);
    glBegin(GL_POINTS);
      for (i = 0; i < this->NumberOfPoint; i++)
			{
				x = this->VControls[3*i];
				y = this->VControls[3*i+1];
				z = 0.5;//this->VControls[3*i+2];
				//cout<< x << "," << y << ","<<z<<endl;
        glVertex3f( x,y,z );
			}
    glEnd();

		glBegin(GL_LINE_STRIP);
      for (i = 0; i < this->NumberOfPoint; i++)
			{
				x = this->VControls[3*i];
				y = this->VControls[3*i+1];
				z = 0.5;//this->VControls[3*i+2];
				//cout<< x << "," << y << ","<<z<<endl;
        glVertex3f( x,y,z );
			}
    glEnd();
/*
		//Left side + 90 degree
		glColor3f(0.0, 1.0, 0.0);
		glBegin(GL_LINE_STRIP);
      for (i = 0; i < this->NumberOfPoint; i++)
			{
				if(i!=(this->NumberOfPoint-1))
				{
					degree = this->findDegree(this->VControls[3*i],this->VControls[3*i+1],this->VControls[3*(i+1)],this->VControls[3*(i+1)+1]);
					//cout<<"Degree :"<<degree<<endl;
				}

				if(degree != -999)
				{
					//cout<<"Degree :"<<degree+90<<endl;
					//cout<<"Degree :"<<this->RWidth/2<<endl;
					//x = this->VControls[3*i]+(this->RWidth/2)*(cos((degree+90)*PI/180));
					//y = this->VControls[3*i+1]+(this->RWidth/2)*(sin((degree+90)*PI/180));
					//z = 0.5;//this->VControls[3*i+2];
					//cout<< x << "," << y << ","<<z<<endl;
					
					this->LeftControls[3*i] = this->VControls[3*i]+(this->RWidth/2)*(cos((degree+90)*PI/180));
					this->LeftControls[3*i+1] = this->VControls[3*i+1]+(this->RWidth/2)*(sin((degree+90)*PI/180));
					this->LeftControls[3*i+2] = 0.5;//this->VControls[3*i+2];
					
					glVertex3f( this->LeftControls[3*i],this->LeftControls[3*i+1],this->LeftControls[3*i+2] );
				}else
				{
					cout << "Degree Error At Road" << endl;
				}


				//x = this->VControls[3*i];
				//y = this->VControls[3*i+1];
				//z = 0.5;//this->VControls[3*i+2];
				//cout<< x << "," << y << ","<<z<<endl;
        //glVertex3f( x,y,z );
			}
    glEnd();

		//Right side - 90 degree
		glColor3f(0.0, 0.0, 1.0);
		glBegin(GL_LINE_STRIP);
      for (i = 0; i < this->NumberOfPoint; i++)
			{
				if(i!=(this->NumberOfPoint-1))
				{
					degree = this->findDegree(this->VControls[3*i],this->VControls[3*i+1],this->VControls[3*(i+1)],this->VControls[3*(i+1)+1]);
					//cout<<"Degree :"<<degree<<endl;
				}

				if(degree != -999)
				{
					//cout<<"Degree :"<<degree+90<<endl;
					//cout<<"Degree :"<<this->RWidth/2<<endl;
					//x = this->VControls[3*i]+(this->RWidth/2)*(cos((degree-90)*PI/180));
					//y = this->VControls[3*i+1]+(this->RWidth/2)*(sin((degree-90)*PI/180));
					//z = 0.5;//this->VControls[3*i+2];
					//cout<< x << "," << y << ","<<z<<endl;
					
					this->RightControl[3*i] = this->VControls[3*i]+(this->RWidth/2)*(cos((degree-90)*PI/180));
					this->RightControl[3*i+1] = this->VControls[3*i+1]+(this->RWidth/2)*(sin((degree-90)*PI/180));
					this->RightControl[3*i+2] = 0.5;//this->VControls[3*i+2];
					
					glVertex3f( this->RightControl[3*i],this->RightControl[3*i+1],this->RightControl[3*i+2] );
				}else
				{
					cout << "Degree Error At Road" << endl;
				}


				//x = this->VControls[3*i];
				//y = this->VControls[3*i+1];
				//z = 0.5;//this->VControls[3*i+2];
				//cout<< x << "," << y << ","<<z<<endl;
        //glVertex3f( x,y,z );
			}
    glEnd();
		*/

		glColor3f(0.0, 0.0, 1.0);
		glBegin(GL_TRIANGLE_STRIP);//GL_LINE_STRIP);//GL_TRIANGLE_STRIP);
			for(i = 0; i < this->NumberOfPoint; i++)
			{
				//glVertex3f( this->LeftControls[3*i],this->LeftControls[3*i+1],this->LeftControls[3*i+2] );
				//glVertex3f( this->RightControl[3*i],this->RightControl[3*i+1],this->RightControl[3*i+2] );
				
				if(i+1 == this->NumberOfPoint)
				{
					glVertex3f( this->LeftControls[3*i],this->LeftControls[3*i+1],this->LeftControls[3*i+2] );
				  glVertex3f( this->RightControls[3*i],this->RightControls[3*i+1],this->RightControls[3*i+2] );
				  break;
				}

				for(j=0;j <= 1;j=j+(1.0/SEGMENT))
				{
					XLeft = (1-j)*(this->LeftControls[3*i])+(j)*(this->LeftControls[3*(i+1)]);
					XRight = (1-j)*(this->RightControls[3*i])+(j)*(this->RightControls[3*(i+1)]);

					YLeft = (1-j)*(this->LeftControls[3*i+1])+(j)*(this->LeftControls[3*(i+1)+1]);
					YRight = (1-j)*(this->RightControls[3*i+1])+(j)*(this->RightControls[3*(i+1)+1]);

					//ZLeft = (1-j)*(this->LeftControls[3*i+2])+(j)*(this->LeftControls[3*(i+1)+2+1]);
					//ZRight = (i-j)*(this->RightControls[3*i+2])+(j)*(this->RightControls[3*(i+1)+2+1]);
					ZLeft = (0.5);
					ZRight = (0.5);
					

					glVertex3f( XLeft,YLeft, ZLeft );
					glVertex3f( XRight,YRight,ZRight );
				}
			}

		glEnd();
		


	glPopMatrix();

}

Road::Render(float x,float y,float z)
{
	float cx,cy,cz;
	cx = x - (this->WidthSection/2);; 
	cy = y - (this->HightSection/2);
	cz = z;

	//this->CalcControlPoints();

	glPushMatrix();
	//glRotatef(90.0, 1.0, 0.0, 0.0);
	glTranslatef(cx,cy,cz);
	this->drawRoad();
	glPopMatrix();
}


Road::getRtype()
{
	return ( this->RType );
}

float Road::findDegree(float x_start,float y_start,float x_stop,float y_stop)
{
	/*float dx=x_stop-x_start;
	float dy=y_stop-y_start;

	float distance = sqrt ( dx * dx + dy * dy );
	
	cout<<"Dx : "<<dx<<", Dy : "<<dy << ", Distance :"<< distance<<endl;

	float angle = acos(dy/distance);

	cout<<"Angle : "<<angle<<endl;
	angle = angle*180/PI;
	return (angle);
	*/

	//cout<<"X1,Y1 : "<<x_start<<","<<y_start<<endl;
	//cout<<"X2,Y2 : "<<x_stop<<","<<y_stop<<endl;

	float dx=x_stop-x_start;
	float dy=y_stop-y_start;

	float angle,Tangle;

	//ABS Float
	if(dx < 0) dx = -1*dx;
	if(dy < 0) dy = -1*dy; 
	//cout<<"Dx : "<<dx<<", Dy : "<<dy <<endl;
	

	//For x
	if(dy==0 && dx ==0 )
	{
		angle = -999; //ERROR SAME POINT
		return angle;
	}

	if(dy==0 && x_start < x_stop)
	{
		angle = 0;
		return (angle*180/PI);
	}else if(dy==0 && x_start > x_stop)
	{
		angle = PI;
		return (angle*180/PI);
	}

	Tangle = atan(dx/dy);

	//If Y2 > Y1 Then TempAngle = 3.14159265358979 - TempAngle
	if(y_stop > y_start)
	{
		Tangle = PI - Tangle;
	}
	//If X2 < X1 Then TempAngle = -TempAngle
	if(x_stop < x_start)
	{
		Tangle = -1*Tangle;
	}
	//TempAngle = 1.5707963267949 - TempAngle
  Tangle = 1.5707963267949 - Tangle;
	//If TempAngle < 0 Then TempAngle = 6.28318530717959 + TempAngle
	if (Tangle < 0 )
	{
		Tangle = 6.28318530717959 + Tangle;
	}
  angle = Tangle;

	//float angle = acos(dy/distance);

	//cout<<"Angle : "<<angle<<endl;
	angle = angle*180/PI;
	//return (angle);
	return (360-angle);
}

/////////////////////////////////////////////////////////////////////////////////
// Set File Path
/////////////////////////////////////////////////////////////////////////////////

/*Road::setFilePath(char *path)
{
	this->NFControlpoints = path;
}*/
